<?xml version="1.0" ?>
<sdf version='1.6'>
  <!--
      This adds a model of the micro underwater robot HippoCampus, developed at the Institute of Mechanics and Ocean Engineering at TU Hamburg, Germany.
      The model is based on the publications:
      A. Hackbarth, E. Kreuzer, and E. Solowjow. HippoCampus: A Micro Underwater Vehicle for Swarm Applications. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Hamburg, Germany, 2015
      D.-A. Duecker, A. Hackbarth, T. Johannink, E. Kreuzer, and E. Solowjow. Micro Underwater Vehicle Hydrobatics: A Submerged Furuta Pendulum. In IEEE International Conference on Robotics and Automation (ICRA), Brisbane, Australia, 2018

      Please find more information under https://www.tuhh.de/mum/forschung/forschungsgebiete-und-projekte/flow-field-estimation-with-a-swarm-of-auvs.html

      The following model description starts with the first link, the base (hippocampus without rotors). The pose is relative to the given frame.
      The hydrodynamic effects are modelled as forces/moments, see 'UpdateForcesAndMoments'-method in the uuv plugin.
  -->
  <model name='uuv_hippocampus'>
    <static>0</static>
    <link name='base_link'>

      <pose>0 0 0 0 0 0</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <!--
          These values are a compromise between the dry mass and added mass
          values used to correct for hydrodynamic effects.
          Real values:  m = 1,47 kg; Ixx = 0,002408 kgm2; Iyy = Izz = 0.010717 kgm2
                        Xu = -1,11 kg; Yv = Zw = -2,8 kg; Kp = -0,00451 kgm2; Mq = Nr = -0,0163 kgm2
        -->
        <mass>1.47</mass>
        <inertia>
          <ixx>0.0024</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.010717</iyy>
          <iyz>0</iyz>
          <izz>0.010717</izz>
        </inertia>
      </inertial>
      <collision name='base_link_inertia_collision'>
        <pose>0 0 0 0 1.570796 0</pose>
        <geometry>
          <cylinder>
            <radius>0.03</radius>
            <length>0.36</length>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode>
              <min_depth>0.001</min_depth>
              <max_vel>0</max_vel>
            </ode>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='base_link_inertia_visual'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://uuv_hippocampus/meshes/uuv_hippocampus.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Grey</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <velocity_decay/>
    </link>

    <!--
        A new link is defined which hold the IMU (e.g. acceleration sensors, ...). For the simulation it is positioned
        at the center of the Hippocampus.
        The orientation of the IMU is chosen in regard to the orientation of the Body coordinate system of the
        HippoCampus. PX4 uses the data from the IMU to calculate the orientation. Therefore PX4 assumes a NED (North-
        East-Down) coordinate frame whereas Gazebo uses a ENU (East-North-Up) coordinate frame.
    -->
<!-- IMU link -->
    <link name='uuv_hippocampus/imu_link'>
      <pose>0 0 0 0 0 0</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.015</mass>
        <inertia>
          <ixx>1e-05</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>1e-05</iyy>
          <iyz>0</iyz>
          <izz>1e-05</izz>
        </inertia>
      </inertial>
    </link>

<!-- IMU joint -->
    <joint name='uuv_hippocampus/imu_joint' type='revolute'>
      <child>uuv_hippocampus/imu_link</child>
      <parent>base_link</parent>
      <axis>
        <xyz>1 0 0</xyz>
        <limit>
          <lower>0</lower>
          <upper>0</upper>
          <effort>0</effort>
          <velocity>0</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>

<!--
    Now the rotors are defined on which the forces and moments attack. They are positioned relative to the center
    of the Hippocampus. Also a collision cylinder will be defined for each rotor to reduce computation time.
-->
<!-- CCW 1-->
<!-- rotor_0 link -->
    <link name='rotor_0'>
      <pose>-0.05 0.0481 0.0481 0 1.570796 0</pose>
      <inertial>
        <pose>0 0 0 0 -0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <collision name='rotor_0_collision'>
        <pose>0 0 0.02 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.06</length>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_0_visual'>
        <pose>0 0 0 0 -1.570796 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://uuv_hippocampus/meshes/uuv_hippocampus_prop.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Red</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <velocity_decay/>
    </link>

<!-- rotor_0 joint -->
    <joint name='rotor_0_joint' type='revolute'>
      <parent>base_link</parent>
      <child>rotor_0</child>
      <axis>
        <xyz>1 0 0</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>

<!-- CW 2 -->
<!-- rotor_1 link -->
    <link name='rotor_1'>
      <pose>-0.05 -0.0481 0.0481 0 1.570796 0</pose>
      <inertial>
        <pose>0 0 0 0 -0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <collision name='rotor_1_collision'>
        <pose>0 0 0.02 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.06</length>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_1_visual'>
        <pose>0 0 0 0 -1.570796 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://uuv_hippocampus/meshes/uuv_hippocampus_prop.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Green</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <velocity_decay/>
    </link>

<!-- rotor_1 joint -->
    <joint name='rotor_1_joint' type='revolute'>
    <child>rotor_1</child>
      <parent>base_link</parent>
      <axis>
        <xyz>1 0 0</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>

<!-- CCW 3 -->
<!-- rotor_2 link -->
    <link name='rotor_2'>
      <pose>-0.05 -0.0481 -0.0481 0 1.570796 0</pose>
      <inertial>
        <pose>0 0 0 0 -0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <collision name='rotor_2_collision'>
        <pose>0 0 0.02 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.06</length>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_2_visual'>
        <pose>0 0 0 0 -1.570796 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://uuv_hippocampus/meshes/uuv_hippocampus_prop.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <velocity_decay/>
    </link>

<!-- rotor_2 joint -->
    <joint name='rotor_2_joint' type='revolute'>
      <child>rotor_2</child>
      <parent>base_link</parent>
      <axis>
        <xyz>1 0 0</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>

<!-- CW 4 -->
<!-- rotor_3 link -->
    <link name='rotor_3'>
      <pose>-0.05 0.0481 -0.0481 0 1.570796 0</pose>
      <inertial>
        <pose>0 0 0 0 -0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <collision name='rotor_3_collision'>
        <pose>0 0 0.02 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.06</length>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_3_visual'>
        <pose>0 0 0 0 -1.570796 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://uuv_hippocampus/meshes/uuv_hippocampus_prop.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <velocity_decay/>
    </link>

<!-- rotor_3 joint -->
    <joint name='rotor_3_joint' type='revolute'>
      <child>rotor_3</child>
      <parent>base_link</parent>
      <axis>
        <xyz>1 0 0</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>

<!-- GPS -->
    <include>
      <uri>model://gps</uri>
      <pose>0 0 0 0 0 0</pose>
      <name>gps</name>
    </include>
    <joint name='gps_joint' type='fixed'>
      <child>gps::link</child>
      <parent>base_link</parent>
    </joint>

<!-- Plugins -->

    <plugin name='groundtruth_plugin' filename='libgazebo_groundtruth_plugin.so'>
      <robotNamespace/>
    </plugin>

    <plugin name='uuv_forces' filename='libgazebo_uuv_plugin.so'>
      <buoyancy>
        <link_name>base_link</link_name>
        <origin>0 0 0.01</origin>
        <compensation>1.1</compensation>
        <height_scale_limit>0.05</height_scale_limit>
      </buoyancy>
      <robotNamespace/>
      <baseLinkName>base_link</baseLinkName>
      <addedMassLinear>1.11 2.8 2.8</addedMassLinear>
      <addedMassAngular>0.00451 0.0163 0.0163</addedMassAngular>
      <dampingLinear>5.39 17.36 17.36</dampingLinear>
      <dampingAngular>0.00114 0.007 0.007</dampingAngular>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
    </plugin>

    <plugin name='top_port_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <reversible>true</reversible>
      <jointName>rotor_0_joint</jointName>
      <linkName>rotor_0</linkName>
      <turningDirection>ccw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1100</maxRotVelocity>
      <motorConstant>8.5</motorConstant>
      <momentConstant>0.0024</momentConstant>
      <rotorDragCoefficient>0.0</rotorDragCoefficient>
      <rollingMomentCoefficient>0.0</rollingMomentCoefficient>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>0</motorNumber>
      <motorSpeedPubTopic>/motor_speed/0</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>0.05</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='top_starboard_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <reversible>true</reversible>
      <jointName>rotor_1_joint</jointName>
      <linkName>rotor_1</linkName>
      <turningDirection>cw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1100</maxRotVelocity>
      <motorConstant>-8.5</motorConstant>
      <momentConstant>0.0024</momentConstant>
      <rotorDragCoefficient>0.0</rotorDragCoefficient>
      <rollingMomentCoefficient>0.0</rollingMomentCoefficient>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>1</motorNumber>
      <motorSpeedPubTopic>/motor_speed/1</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>0.05</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='bottom_starboard_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <reversible>true</reversible>
      <jointName>rotor_2_joint</jointName>
      <linkName>rotor_2</linkName>
      <turningDirection>ccw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1100</maxRotVelocity>
      <motorConstant>8.5</motorConstant>
      <momentConstant>0.0024</momentConstant>
      <rotorDragCoefficient>0.0</rotorDragCoefficient>
      <rollingMomentCoefficient>0.0</rollingMomentCoefficient>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>2</motorNumber>
      <motorSpeedPubTopic>/motor_speed/2</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>0.05</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='bottom_port_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <reversible>true</reversible>
      <jointName>rotor_3_joint</jointName>
      <linkName>rotor_3</linkName>
      <turningDirection>cw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1100</maxRotVelocity>
      <motorConstant>-8.5</motorConstant>
      <momentConstant>0.0024</momentConstant>
      <rotorDragCoefficient>0.0</rotorDragCoefficient>
      <rollingMomentCoefficient>0.0</rollingMomentCoefficient>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>3</motorNumber>
      <motorSpeedPubTopic>/motor_speed/3</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>0.05</rotorVelocitySlowdownSim>
    </plugin>

    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
      <robotNamespace/>
      <pubRate>100</pubRate>
      <noiseDensity>0.0004</noiseDensity>
      <randomWalk>6.4e-06</randomWalk>
      <biasCorrelationTime>600</biasCorrelationTime>
      <magTopic>/mag</magTopic>
    </plugin>

    <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
      <robotNamespace/>
      <pubRate>50</pubRate>
      <baroTopic>/baro</baroTopic>
    </plugin>

    <plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
      <robotNamespace/>
      <imuSubTopic>/imu</imuSubTopic>
      <magSubTopic>/mag</magSubTopic>
      <baroSubTopic>/baro</baroSubTopic>
      <mavlink_addr>INADDR_ANY</mavlink_addr>
      <mavlink_udp_port>14560</mavlink_udp_port>
      <mavlink_tcp_port>4560</mavlink_tcp_port>
      <serialEnabled>0</serialEnabled>
      <serialDevice>/dev/ttyACM0</serialDevice>
      <baudRate>921600</baudRate>
      <qgc_addr>INADDR_ANY</qgc_addr>
      <qgc_udp_port>14550</qgc_udp_port>
      <sdk_addr>INADDR_ANY</sdk_addr>
      <sdk_udp_port>14540</sdk_udp_port>
      <hil_mode>false</hil_mode>
      <hil_state_level>false</hil_state_level>
      <send_vision_estimation>true</send_vision_estimation>
      <send_odometry>false</send_odometry>
      <enable_lockstep>true</enable_lockstep>
      <use_tcp>true</use_tcp>
      <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>

      <!-- control channels, this way for every channel different settings can be realized -->
      <control_channels>
        <channel name='rotor1'>
          <input_index>0</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
          <joint_name>top_port_motor_joint</joint_name>
        </channel>
        <channel name='rotor2'>
          <input_index>1</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
          <joint_name>top_starboard_joint</joint_name>
        </channel>
        <channel name='rotor3'>
          <input_index>2</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
          <joint_name>bottom_starboard_joint</joint_name>
        </channel>
        <channel name='rotor4'>
          <input_index>3</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
          <joint_name>bottom_port_joint</joint_name>
        </channel>
      </control_channels>
    </plugin>

    <plugin name='gazebo_imu_plugin' filename='libgazebo_imu_plugin.so'>
      <robotNamespace/>
      <linkName>uuv_hippocampus/imu_link</linkName>
      <imuTopic>/imu</imuTopic>
      <gyroscopeNoiseDensity>0.0003394</gyroscopeNoiseDensity>
      <gyroscopeRandomWalk>3.8785e-05</gyroscopeRandomWalk>
      <gyroscopeBiasCorrelationTime>1000.0</gyroscopeBiasCorrelationTime>
      <gyroscopeTurnOnBiasSigma>0.0087</gyroscopeTurnOnBiasSigma>
      <accelerometerNoiseDensity>0.004</accelerometerNoiseDensity>
      <accelerometerRandomWalk>0.006</accelerometerRandomWalk>
      <accelerometerBiasCorrelationTime>300.0</accelerometerBiasCorrelationTime>
      <accelerometerTurnOnBiasSigma>0.196</accelerometerTurnOnBiasSigma>
    </plugin>
  </model>
</sdf>
